#pragma once
#include <stdio.h>
#include <intrin.h>
#include <smmintrin.h>

#include<string>
#include<map>
#include<vector>

#include "Shape/PointCloud.h"
#include "Geometry/Vector3.h"
#include "Geometry/BoundingBox.h"
#include "System/Platform.h"

#include <Eigen/Dense>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/timer.hpp>
#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <boost/lexical_cast.hpp>

namespace PCS
{
	namespace Core
	{     	

		struct statOOCMMF {
				int threadId;
				float elapsed;
		};

		struct cellElementNMMF {
				grid_key key;
				size_t offset;
		};

		struct cell_info {
				size_t count;
				size_t idx;		
				size_t offset;
		};

		struct grid_range {
				size_t x_from;
				size_t x_to;
				size_t p_count;
				size_t c_count;
				grid_key start;				
				std::string file_path;
		};

		class SparseGridNMMFs
		{
		protected:
			Vector3 m_size;											//number of cells along the three axis of the grid.
			Vector3 m_resolution;									//resolution along the X,Y,Z axis of the grid
			Vector3 m_extent;
			AxisAlignedBoundingBox * m_pBounds;						//bounding volume for the point cloud assigned to this space ... not necessarily the same as the point cloud volume.
			std::map<grid_key, std::vector<size_t>*> * m_pGrid;		//map data structure used to store points in cloud			
			std::map<grid_key, cell_info> pGridInfo;				//map data structure used to store minimal info (count + offset) per cell in cloud
			PointCloud * m_pointCloud;
			float m_buffer;											//expand this sparse grid bounding volume by this buffer 
			float m_neighbourRadiusSq;					 

			float m_extent_x, m_extent_y, m_extent_z;
			size_t totalPoints;

			int averageBinSize;
			int minBinSize;
			int maxBinSize;

			float r_average;
			
			std::vector<grid_range*> clusters;
			float * r_addr;

		public:			
			SparseGridNMMFs(void) { m_resolution = 10.0f; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGridNMMFs(float p_resolution) { m_resolution = p_resolution; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGridNMMFs(Vector3 * p_resolution) { m_resolution = *p_resolution; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGridNMMFs(float p_resolution, float p_buffer) { m_resolution = p_resolution; m_buffer = p_buffer; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }

			~SparseGridNMMFs() 
			{
				delete m_pGrid;
				delete m_pointCloud;
			}

			inline static unsigned int gridHash(int x, int y, int z) {
				return ((x & 0x3FF) << 20) | ((y & 0x3FF) << 10) | (z & 0x3FF);
			}

			AxisAlignedBoundingBox * GetBounds() {
				return m_pBounds;
			}

			int GetAverageBinSize() {
				return averageBinSize;
			}

			int GetMinBinSize() {
				return minBinSize;
			}

			int GetMaxBinSize() {
				return maxBinSize;
			}

			Vector3 GetSize() {
				return m_size;
			}

			Vector3 GetResolution() {
				return m_resolution;
			}

			Vector3 GetExtent() {
				return m_extent;
			}

			int getTotalPoints() {
				return totalPoints;
			}

			std::map<grid_key, std::vector<size_t>*> * GetMap() {
				return m_pGrid;
			}

			PCS::Core::PointCloud * GetPointCloud() {
				return m_pointCloud;
			}			

			void SetBoundsWithBuffer(AxisAlignedBoundingBox * p_bounds) 
			{ 
				Vector3 gridBuffer = p_bounds->GetExtent() * 2 * m_buffer;
				m_pBounds = new AxisAlignedBoundingBox(p_bounds->GetMinExtent()-gridBuffer, p_bounds->GetMaxExtent()+gridBuffer); 
			} 

			void SetBoundsNoBuffer(AxisAlignedBoundingBox * p_bounds) 
			{ 
				m_pBounds = new AxisAlignedBoundingBox(p_bounds->GetMinExtent(), p_bounds->GetMaxExtent()); 
			} 								

			/**Generate the sparse grid structure and insert all point within. Then update point type.*/
			int Initialise(PointCloud* &p_cloud, float p_histogramDensity, unsigned int p_mem_upperbound) 
			{				
				boost::timer timer;				

				m_pointCloud = p_cloud;

				//std::map<grid_key, std::vector<size_t>*> pGrid = std::map<grid_key, std::vector<size_t>*>();
				pGridInfo = std::map<grid_key, cell_info>();
				clusters = std::vector<grid_range*>();

				SetBoundsWithBuffer((AxisAlignedBoundingBox*)(p_cloud->GetBoundingVolume()));
				m_extent.X = (m_pBounds->GetMaxExtent().X) - (m_pBounds->GetMinExtent().X); 
				m_extent.Y = (m_pBounds->GetMaxExtent().Y) - (m_pBounds->GetMinExtent().Y);
				m_extent.Z = (m_pBounds->GetMaxExtent().Z) - (m_pBounds->GetMinExtent().Z);	
				m_extent.W = 0.0f;

				//Now determine number of cells across the X, Y and Z dimensions. Static for now.
				m_size.X = m_extent.X / m_resolution.X;
				m_size.Y = m_extent.Y / m_resolution.Y;
				m_size.Z = m_extent.Z / m_resolution.Z;
				m_size.W = 0.0f;
				std::cout << "[SparseGridNMMF] Number of cells along X, Y and Z = " << m_size.ToString() << std::endl;

				//Set neighbourdistance as the average of the resolutions and divide by 2 ... heuristic.
				m_neighbourRadiusSq = ((m_resolution.X + m_resolution.Y + m_resolution.Z) / 2) * ((m_resolution.X + m_resolution.Y + m_resolution.Z) / 2);				

				//Now iterate over the points in the cloud and insert them in their respective sgrid cells							
				std::vector<size_t>* v;				
				std::map<grid_key, cell_info>::iterator gridInfoIterator;
				std::map<grid_key, std::vector<size_t>*>::iterator gridIterator;				

				grid_key t_key;
				int xIdx, yIdx, zIdx;				
				int xIdxOld, yIdxOld, zIdxOld;				
				int bins = 0;			
				r_average = 0.0f;
				averageBinSize = 0;
				minBinSize = 100;
				maxBinSize = 0;
				xIdxOld = yIdxOld = zIdxOld = -1000;
				__m128 a,b,c,d;

				b = _mm_load_ps(m_pBounds->GetMinExtent().Element);
				c = _mm_load_ps(m_size.Element);
				d = _mm_load_ps(m_extent.Element);																				
													
				size_t idx = 0;
				
				Vector3 crt_point;
				while (p_cloud->GetNext(crt_point))
				{		
					//Implement the following using SSE intrinsics
					//xIdx = (int)floor((float)((((*pointIterator)->position.X - m_pBounds->GetMinExtent().X) * m_size.X) / m_extent.X));
					//yIdx = (int)floor((float)((((*pointIterator)->position.Y - m_pBounds->GetMinExtent().Y) * m_size.Y) / m_extent.Y));
					//zIdx = (int)floor((float)((((*pointIterator)->position.Z - m_pBounds->GetMinExtent().Z) * m_size.Z) / m_extent.Z));									
					
					a = _mm_load_ps(crt_point.Element); 
					a = _mm_sub_ps(a, b);
					a = _mm_mul_ps(a, c);
					a = _mm_div_ps(a, d);
					a = _mm_floor_ps(a);
					
					xIdx = a.m128_f32[0];
					yIdx = a.m128_f32[1];
					zIdx = a.m128_f32[2];

					//If indices have not changed, insert in the same grid cell.
					if (xIdx==xIdxOld && yIdx == yIdxOld && zIdx == zIdxOld) {
						gridInfoIterator->second.count+=1;
						if (gridInfoIterator->second.count > maxBinSize) maxBinSize = gridInfoIterator->second.count;
						if (gridInfoIterator->second.count < minBinSize) minBinSize = gridInfoIterator->second.count;
						continue;
					}

					//Insert point in grid at the computed key.					
					if ((gridInfoIterator = pGridInfo.find(gridHash(xIdx, yIdx, zIdx))) == pGridInfo.end())
					{
						t_key = gridHash(xIdx, yIdx, zIdx);						
						cell_info ci = cell_info();
						ci.count = 1;
						ci.offset = 0;
						(pGridInfo)[t_key] = ci;						
						gridInfoIterator = pGridInfo.find(t_key);
						bins++;
					}
					else
					{
						gridInfoIterator = pGridInfo.find(gridHash(xIdx, yIdx, zIdx));
						gridInfoIterator->second.count+=1;
						if (gridInfoIterator->second.count > maxBinSize) maxBinSize = gridInfoIterator->second.count;
						if (gridInfoIterator->second.count < minBinSize) minBinSize = gridInfoIterator->second.count;
					}
					
					xIdxOld = xIdx;
					yIdxOld = yIdx;
					zIdxOld = zIdx;
				}

				totalPoints = p_cloud->GetSize();				
				averageBinSize = totalPoints / bins;											
				std::cout << boost::str(boost::format("[SparseGridNMMF] InfoGrid Created in %ds. #=%d, avg=%d max=%d min=%d.") % timer.elapsed() % bins % averageBinSize % maxBinSize % minBinSize) << std::endl;			

				std::vector<int> cellCountPerXSlice = std::vector<int>();
				std::vector<int> pointCountPerXSlice = std::vector<int>();
				int crtXCellIdx = 0;
				int prevXCellIdx = 0;				
				gridInfoIterator = pGridInfo.begin();
				prevXCellIdx = gridHashX(gridInfoIterator->first);
				
				int clusterCount = 0;
				int pointCount = 0;
				for (gridInfoIterator = pGridInfo.begin(); gridInfoIterator != pGridInfo.end(); gridInfoIterator++) {					
					crtXCellIdx = gridHashX(gridInfoIterator->first);
					if (prevXCellIdx != crtXCellIdx) 
					{						
						std::cout << "[SparseGridNMMF] Slice XIdx= " << prevXCellIdx << " Count=" << clusterCount << " Points=" << pointCount << std::endl;
						cellCountPerXSlice.push_back(clusterCount);
						pointCountPerXSlice.push_back(pointCount);
						prevXCellIdx = crtXCellIdx;
						clusterCount = 1;
						pointCount = gridInfoIterator->second.count;
					}
					else 
					{						
						clusterCount++;
						pointCount += gridInfoIterator->second.count;						
					}
				}
				//Add the last slice once out of the loop
				cellCountPerXSlice.push_back(clusterCount);
				pointCountPerXSlice.push_back(pointCount);
				std::cout << "[SparseGridNMMF] Slice XIdx= " << prevXCellIdx << " Count=" << clusterCount << " Points=" << pointCount << std::endl;

				//Now re-iterate over the grid and create a cluster list which will be dumped to file then loaded one-by-one							
				unsigned int crt_memory_usage = 0;
				bool just_added_cluster = true;
				std::vector<grid_key> visitedPrevXStartKey = std::vector<grid_key>();
				grid_key prevXStartKey;
				grid_range * crt_grid_range = new grid_range();
				gridInfoIterator = pGridInfo.begin();
				crtXCellIdx = gridHashX(gridInfoIterator->first);
				prevXCellIdx = crtXCellIdx;
				prevXStartKey = gridInfoIterator->first;
				crt_grid_range->x_from = crtXCellIdx;
				crt_grid_range->start = gridInfoIterator->first;
				visitedPrevXStartKey.push_back(gridInfoIterator->first);
				crt_grid_range->c_count = 0;
				crt_grid_range->p_count = 0;
				int buffer_memory = 0;
				int cX = 0; int cXPts; int pXPts; int nXPts;
				cXPts = pointCountPerXSlice.at(0); 
				pXPts = 0;
				nXPts = 0;
				if (pointCountPerXSlice.size() > 1) nXPts = pointCountPerXSlice.at(1);
				//Buffer memory represents the number of points in the slices before and after a given cluster
				buffer_memory = pXPts + nXPts;

				for (gridInfoIterator = pGridInfo.begin(); gridInfoIterator != pGridInfo.end(); gridInfoIterator++) {					
					crtXCellIdx = gridHashX(gridInfoIterator->first);
					if (prevXCellIdx != crtXCellIdx) {						
						//processing new slice
						cX++; //increment current X slice index
						if (cX < pointCountPerXSlice.size() - 2) 
						{ 
							//still many slices to process ...
							nXPts = pointCountPerXSlice.at(cX+1);
							nXPts += pointCountPerXSlice.at(cX);
						}
						else 
						{
							if (cX < pointCountPerXSlice.size() - 1)
							{
								//last slice ....
								nXPts = pointCountPerXSlice.at(cX);
							}
							else 
							{
								//We're at the end ... 
								nXPts = 0;
							}
						}
						
						buffer_memory = pXPts + nXPts;
						
						//std::cout << "Checking Slice " << cX << " CrtMemUsage = " << crt_memory_usage << " BuffMem = " << buffer_memory << " MemBound = " <<  p_mem_upperbound << std::endl;
						if (crt_memory_usage + (buffer_memory) > p_mem_upperbound)
						{							
							//we're done with this cluster ... finalise the record and move on
							crt_grid_range->x_to = prevXCellIdx;
							crt_grid_range->c_count+=cellCountPerXSlice.at(cX); //add the cells in this slice (buffer)							
							crt_grid_range->file_path = "..filepath";
							clusters.push_back(crt_grid_range);
							std::cout << "[SparseGridNMMF] Cluster Created: xFrom=" << crt_grid_range->x_from << " xTo=" << crt_grid_range->x_to << " #p= " << crt_grid_range->p_count << " #c= " << crt_grid_range->c_count << std::endl; 
							pXPts = pointCountPerXSlice.at(cX-1);
							//start a new cluster
							crt_grid_range = new grid_range();
							crt_grid_range->x_from = crtXCellIdx;
							crt_grid_range->c_count = 1;
							crt_grid_range->c_count += cellCountPerXSlice.at(cX-1);
							crt_grid_range->p_count = gridInfoIterator->second.count;
							crt_grid_range->start = visitedPrevXStartKey.at(visitedPrevXStartKey.size()-1);
							crt_memory_usage = gridInfoIterator->second.count;
						}
						else 
						{
							//continue adding to the previous cluster ... looks like there's enough mem to allocate
							crt_memory_usage += gridInfoIterator->second.count;
							crt_grid_range->c_count++;
							crt_grid_range->p_count+=gridInfoIterator->second.count;
						}
						visitedPrevXStartKey.push_back(gridInfoIterator->first);
						prevXCellIdx = crtXCellIdx;
					} else 
					{
						//still on the same X slice keep on adding to this cluster
						crt_memory_usage += gridInfoIterator->second.count;
						crt_grid_range->c_count++;
						crt_grid_range->p_count+=gridInfoIterator->second.count;
					}
				}				

				crt_grid_range->x_to = crtXCellIdx;                
				crt_grid_range->file_path = "..filepath";
				clusters.push_back(crt_grid_range);
				std::cout << "[SparseGridNMMF] Cluster Created: xFrom=" << crt_grid_range->x_from << " xTo=" << crt_grid_range->x_to << " #p=" << crt_grid_range->p_count << " #c=" << crt_grid_range->c_count << std::endl; 							
				
				//now iterate over them and persist a cluster/file
				boost::interprocess::mapped_region *m_region;
				boost::interprocess::file_mapping m_file;
				boost::timer timer0;				
				bool flushed;
				for (int crtCluster = 0; crtCluster<clusters.size(); crtCluster++) 
				{
					timer0.restart();
					//set up offsets in the gridInfo cells for the cluster
					flushed = false;
					crt_grid_range = clusters[crtCluster];
					gridInfoIterator = pGridInfo.find(crt_grid_range->start);
					size_t crt_offset = 0;					
					for (int i = 0; i<crt_grid_range->c_count; i++) 
					{
						gridInfoIterator->second.offset = crt_offset;
						gridInfoIterator->second.idx = 0;
						crt_offset += (gridInfoIterator->second.count * 3);
						gridInfoIterator++;
					}

					std::string s0 = boost::lexical_cast<std::string>(crt_grid_range->x_from);
					std::string s1 = boost::lexical_cast<std::string>(crt_grid_range->x_to);
					std::string newfile = m_pointCloud->GetFilepath().insert(m_pointCloud->GetFilepath().size()-4, "_"+s0+"-"+s1);
					crt_grid_range->file_path = newfile;
					const char * filename = newfile.c_str();	
					size_t points_to_write = crt_offset / 3;

					//Create File
					std::filebuf fbuf;
					fbuf.open(filename, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
					fbuf.pubseekoff((crt_offset*sizeof(float))-sizeof(char), std::ios_base::beg);				
					fbuf.sputc(0);
					fbuf.close();

					//create a MMF which will store this cluster
					m_file = boost::interprocess::file_mapping(filename, boost::interprocess::read_write);
					m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_write);
					r_addr = (float*)m_region->get_address();
					//region now mapped to memory ... iterate over point cloud to fill it from xFrom to xTo ...
					m_pointCloud->Reset();
					Vector3 crt_point;
					xIdxOld = yIdxOld = zIdxOld = -1000;
					size_t points_written = 0;
					size_t write_location;
					//std::cout << "Point to write for region [" << s0 << "," << s1 << "] = " << points_to_write << std::endl;
					while (m_pointCloud->GetNext(crt_point))
					{		
						if (points_written == points_to_write) 
						{
							std::cout << "[SparseGridNMMF] Region File Created (" << timer0.elapsed() << ")" << " xRange-> [" << s0 << "," << s1 << "]" << std::endl;
							m_region->flush();
							flushed = true;
							delete m_region;
							break;  // all points written to file .. no need to check the rest of the point cloud
						}

						a = _mm_load_ps(crt_point.Element); 
						a = _mm_sub_ps(a, b);
						a = _mm_mul_ps(a, c);
						a = _mm_div_ps(a, d);
						a = _mm_floor_ps(a);
					
						xIdx = a.m128_f32[0];						
						//persist points from x_from-1 to x_to+1 i.e. including top and bottom boundaries
						if (xIdx >= crt_grid_range->x_from-1 && xIdx <= crt_grid_range->x_to+1) 
						{
							//write this point in this MMF
							yIdx = a.m128_f32[1];
							zIdx = a.m128_f32[2];
							//If indices have not changed, insert in file exactly after the previous point
							if (xIdx==xIdxOld && yIdx == yIdxOld && zIdx == zIdxOld) 
							{
								r_addr[write_location] = crt_point.X; write_location++;
								r_addr[write_location] = crt_point.Y; write_location++;
								r_addr[write_location] = crt_point.Z; write_location++;
								gridInfoIterator->second.idx++; //increment index of current cell
								points_written++; 
								continue;
							}

							//Insert point in grid at the computed key.					
							gridInfoIterator = pGridInfo.find(gridHash(xIdx, yIdx, zIdx));
							write_location = gridInfoIterator->second.offset + (gridInfoIterator->second.idx*3);
							r_addr[write_location] = crt_point.X; write_location++;
							r_addr[write_location] = crt_point.Y; write_location++;
							r_addr[write_location] = crt_point.Z; write_location++;
							gridInfoIterator->second.idx++; //increment index of current cell
							points_written++; 														
							xIdxOld = xIdx;
							yIdxOld = yIdx;
							zIdxOld = zIdx;
						}	
					}
					//Shit we had to read the whole file to get here ... TODO perhaps can start reading from the last segment of the point cloud instead of from the beginning ... implement getPrev in pointcloud class
					if (!flushed && (points_written == points_to_write)) 
					{
							std::cout << "[SparseGridNMMF] Region File Created (" << timer0.elapsed() << ")" << " xRange-> [" << s0 << "," << s1 << "]" << std::endl;
							m_region->flush();
							delete m_region;							
					}
				}

				//Now perform kNN queries on all points in the cloud by iterating over the clusters ...
				boost::timer global_knn;
				boost::timer global_io;
				double io_time = 0;
				for (int crtCluster = 0; crtCluster<clusters.size(); crtCluster++) 
				{
					timer0.restart();
					size_t crt_offset = 0;
					//first recompute local offsets within this region and load corresponding MMF
					gridInfoIterator = pGridInfo.find(clusters[crtCluster]->start);
					for (int i = 0; i<clusters[crtCluster]->c_count; i++) 
					{
						gridInfoIterator->second.offset = crt_offset;
						gridInfoIterator->second.idx = 0;
						crt_offset += (gridInfoIterator->second.count * 3);
						gridInfoIterator++;
					} // offsets recomputed
					global_io.restart();
					m_file = boost::interprocess::file_mapping(clusters[crtCluster]->file_path.c_str(), boost::interprocess::read_only);
					m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only);
					io_time += global_io.elapsed();
					r_addr = (float*)m_region->get_address();
					//float last = r_addr[m_region->get_size - sizeof(float)];
					UpdatePointType_DD(crtCluster);
					delete m_region;
					std::cout << "[SparseGridNMMF] kNN computed on cluster in " << timer0.elapsed() << std::endl;
				}
				std::cout << "[SparseGridNMMF] kNN computed on all clusters in " << global_knn.elapsed() << " Time on IO = " << io_time << std::endl;
				return 0;
			};

			void PrintDensityHistogram(float p_binDensity) {
				float binDensitySpan = GetMaxBinSize() - GetMinBinSize();
				float binWidth = binDensitySpan * p_binDensity;
				int numberOfBins = (int) (binDensitySpan / binWidth);
				std::vector<int> bins = std::vector<int>();
				for (int i = 0; i<numberOfBins+1; i++) bins.push_back(0);
				for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = m_pGrid->begin(); cellIterator != m_pGrid->end(); ++cellIterator) {					

					bins[(int)(cellIterator->second->size()/binWidth)]+=1;
				}
				std::cout << "[SparseGridOOC] Histogram ******" << std::endl; 
				for (int i = 0; i<numberOfBins+1; i++) {
					if (bins[i] != 0)
						std::cout << "[SparseGrid] Bin " << i << " [" << i*binWidth << "-" << (i*binWidth)+binWidth << "] = " << bins[i] << std::endl;
				}
				std::cout << "[SparseGridOOC] Histogram ******" << std::endl; 
			}
			
			/**Iterate over the points in each cell of the grid, setting their type (surface or edge)*/
			void UpdatePointType_DD(int _crtCluster) {
				grid_range * crt_grid_range = clusters[_crtCluster];
				stat * stats = new stat[4];
				cellElementNMMF * cellsInGrid = new cellElementNMMF[crt_grid_range->c_count];
				boost::timer timer;
				int neighbourCount = 0;
				int pointsChecked = 0;
				int notEnoughNeighbours = 0;
				
				int edgePoints = 0;
				int surfacePoints = 0;
				float eigenRatioEdge = 0.0f;

				int numberOfProcs = omp_get_num_procs();				
				std::cout << "[SparseGridNMMF] " << numberOfProcs << " procs computing kNN over " << crt_grid_range->c_count << " cells " << std::endl;
												
				//Initialise shared array of points to the cells in the sgrid								
				int cell_count = 0;
				for (std::map<grid_key, cell_info>::iterator gridInfoIterator = pGridInfo.find(crt_grid_range->start); cell_count<crt_grid_range->c_count; ++gridInfoIterator)
				{
					cellsInGrid[cell_count].key = gridInfoIterator->first;
					cellsInGrid[cell_count].offset = gridInfoIterator->second.offset;					
					cell_count++;
				}								
				int size = cell_count;
				int idx = 0;
				
				#pragma omp parallel for schedule(guided)
				for (int i=0; i<numberOfProcs; i++) {
					float elapsedStart = timer.elapsed();
					int localNeighbourCount = 0;
					int localPointsChecked = 0;
					int localNotEnoughNeighbours = 0;
					int xIdxP; int yIdxP; int zIdxP;
					int localEdgePoints = 0;
					int localSurfacePoints = 0;
					int increaseNeighbours = 0;
					int increaseInNeighbourhoodQueries = 0;
					bool incDone = false;
					int breakIncreaseNeighbours = 0;
					int pointsInBlock = 0;

					float minDistance = 100000;
					float maxDistance = 0;
					float crtDistance = 0;
					float medianDistance = 0;

					Eigen::Matrix3f covariance = Eigen::Matrix3f();				
					Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
					
					std::vector<cell_info> neighbourCells = std::vector<cell_info>();					
					std::vector<size_t> neighbours = std::vector<size_t>();	
					std::vector<size_t> pointList = std::vector<size_t>();
					grid_key cellKey;
					std::map<grid_key, cell_info>::iterator gridInfoIterator;					
					
					int nextCell = 0;
					while (true) 
					{										
						//Determine which adjacent cells need to be considered and pass them to the neighbourhood query
						neighbourCells.clear();						
						nextCell = _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&idx), 1);
						if (nextCell > size-1) 
							break;

						cellKey = cellsInGrid[nextCell].key;						
						if (gridHashX(cellKey) < crt_grid_range->x_from)
							continue;

						if (gridHashX(cellKey) > crt_grid_range->x_to)
							continue;
						
						gridInfoIterator = pGridInfo.find(cellKey);
						
						//first search the voxel this point is located in						
						neighbourCells.push_back(gridInfoIterator->second);
						pointsInBlock = 0; //reset number of points in this block
						//then its adjacent ones
						unsigned int nck_x, nck_y;
						for (int xIdx = -1; xIdx < 2; ++xIdx) {
							nck_x = cellKey + (xIdx << 20);
							for (int yIdx = -1; yIdx < 2; ++yIdx) {
								nck_y = nck_x + (yIdx << 10);
								for (int zIdx = -1; zIdx < 2; ++zIdx) {
									if (!(xIdx==0 && yIdx==0 && zIdx==0)) {										
										if ((gridInfoIterator = pGridInfo.find(nck_y+zIdx)) != pGridInfo.end())	{
											neighbourCells.push_back(gridInfoIterator->second);
											pointsInBlock+=gridInfoIterator->second.count;											
										}
									}
								}
							}
						}

						gridInfoIterator = pGridInfo.find(cellKey);
						
						if (pointsInBlock > KDTREE_THOLD) {
							//create KDTREE on the local set of points and use it for neighbourhood queries							
							pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud (new pcl::PointCloud<pcl::PointXYZ>);
							pointList.clear();
							for (std::vector<cell_info>::iterator cellIterator = neighbourCells.begin(); cellIterator != neighbourCells.end(); ++cellIterator) {
								int idx = cellIterator->offset;
								for (int j = 0; j < cellIterator->count; j++ )
								{								
									pcd_cloud->push_back(pcl::PointXYZ(r_addr[idx], r_addr[idx+1], r_addr[idx+2]));
									idx += 3;
									//pointList.push_back(*pointIterator);								
								}
							}
							pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
							kdtree.setInputCloud(pcd_cloud);
							std::vector<int> pointIdxNKNSearch(N_FOR_TYPE);
							std::vector<float> pointNKNSquaredDistance(N_FOR_TYPE);							
							size_t crtIdx;
							for (int k = 0; k<gridInfoIterator->second.count; k++) 
							{
								kdtree.nearestKSearch (k, N_FOR_TYPE, pointIdxNKNSearch, pointNKNSquaredDistance);
								//neighbours.clear();
								//for (int i = 0; i<N_FOR_TYPE; i++) neighbours.push_back(pointList.at(pointIdxNKNSearch[i]));																		
								pointIdxNKNSearch.clear();
								pointNKNSquaredDistance.clear();
								/*if (neighbours.size() < N_FOR_TYPE) localNotEnoughNeighbours++;
								else 
								{
									crtDistance = Vector3::DistanceSquared(m_pointCloud->GetPointAt(neighbours[N_FOR_TYPE-1])->position, m_pointCloud->GetPointAt(*pointIterator)->position);
									if (crtDistance > maxDistance) maxDistance = crtDistance;
									if (crtDistance < minDistance) minDistance = crtDistance;
									CalcCovMatrix(neighbours, N_FOR_TYPE, covariance);
									eigensolver.compute(covariance);
									if (eigensolver.eigenvalues()[0] * CURVATURE > eigensolver.eigenvalues()[1]) 
									{											
										m_pointCloud->GetPointAt(*pointIterator)->T = edge;
										localEdgePoints++;											
									} 
									else 
									{
										//std::cout << "S- " << eigensolver.eigenvalues()[0] << "(" << eigensolver.eigenvalues()[0] * CURVATURE << ") - " << eigensolver.eigenvalues()[1] << eigensolver.eigenvalues()[2] << std::endl;
										m_pointCloud->GetPointAt(*pointIterator)->T = surface;
										localSurfacePoints++;													
									}
								}*/
								localPointsChecked++;
								if (localPointsChecked % 250000 == 0) 
									std::cout << boost::str(boost::format("[SparseGridNMMF] T%d - #=%d") % omp_get_thread_num() % localPointsChecked) << " -> " << timer.elapsed() << std::endl;
							}							
							continue;
						}						
					}

					std::cout << boost::str(boost::format("[SparseGridNMMF] T%d (%ds) #=%d NEN=%d MaxD=%d MinD=%d IncN=%d IncNQ=%d") % omp_get_thread_num() % timer.elapsed() % localPointsChecked % localNotEnoughNeighbours  % sqrt(maxDistance) % sqrt(minDistance) % increaseNeighbours % increaseInNeighbourhoodQueries) << std::endl;					
					stats[omp_get_thread_num()].threadId = omp_get_thread_num();
					stats[omp_get_thread_num()].elapsed = timer.elapsed() - elapsedStart;
					//write totals to shared variables
					#pragma omp critical(inc_patchid)
					{
						pointsChecked += localPointsChecked;
						edgePoints += localEdgePoints;
						surfacePoints += localSurfacePoints;
						notEnoughNeighbours += localNotEnoughNeighbours;
					}

				}  //end pragma									
				
				delete crt_grid_range;
				std::cout << "[SparseGridNMMF] All threads finished updating point types in " << timer.elapsed() << std::endl;				
				std::cout << boost::str(boost::format("[SparseGridNMMF] Totals #=%d E=%d S=%d NEN=%d") % pointsChecked % edgePoints % surfacePoints % notEnoughNeighbours) << std::endl;
			}

			/**Returns points from the MMF populated in the UpdateDD function*/			
			void GetNeighbours(size_t p_fromPoint, std::vector<size_t> &p_neighbours, size_t * r_addr) {				
				p_neighbours.clear(); //Clear the vector				
				size_t numberOfNeighbours = r_addr[p_fromPoint*(N_FOR_CRAWL+1)];
				//std::cout<< "T" << omp_get_thread_num() << " Point Idx = " << p_fromPoint << " #Neighbours= " << numberOfNeighbours << std::endl;
				for (int i = 1, j=p_fromPoint*(N_FOR_CRAWL+1)+1; i<=numberOfNeighbours; i++, j++) p_neighbours.push_back(r_addr[j]);
			}
			
			/**Returns points within a "reasonable" distance from p_fromPoint located in this and surrounding voxels of the sparse grid*/			
			void GetNeighbours(Point * p_fromPoint, std::vector<std::vector<Point*>*> &p_cellValues, std::vector<Point*> &p_neighbours) {				
				p_neighbours.clear(); //Clear the vector
				for (std::vector<std::vector<Point*>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<Point*>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {
						if (Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position) < m_neighbourRadiusSq) { 
							p_neighbours.push_back(*pointIterator);
						}	
					}
				}
			}						
			
			/**Returns the closest p_numberOfNeighbours from p_fromPoint located in the surrounding voxels of the sparse grid*/
			void GetNeighbours(size_t p_fromPoint, std::vector<std::vector<size_t>*> &p_cellValues, std::vector<Point*> &p_neighbours, int p_numberOfNeighbours) {				
				p_neighbours.clear(); //Clear the list
				std::vector<float> distances = std::vector<float>(p_numberOfNeighbours, 100); 				
				int distIdx = 0;
				float crtDist = 0.0f;				
				__m128 a,b;
				__m128 res;
				const int mask = 0xF8;				
				b = _mm_load_ps(m_pointCloud->GetPointAt(p_fromPoint)->position.Element);
				for (std::vector<std::vector<size_t>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {												
						a = _mm_load_ps(m_pointCloud->GetPointAt(*pointIterator)->position.Element);												
						a = _mm_sub_ps(a, b);
						res = _mm_dp_ps(a, a, mask);
						crtDist = res.m128_f32[3];						
						if (crtDist < distances[p_numberOfNeighbours-1]) {
							for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
								if (crtDist <= distances[distIdx]) {								
									distances.insert(distances.begin()+distIdx, crtDist);									
									p_neighbours.insert(p_neighbours.begin()+distIdx, m_pointCloud->GetPointAt(*pointIterator));									
									break;
								}								
							}
						}						
					}
				}				
			}

			/**Returns the closest p_numberOfNeighbours from p_fromPoint located in the surrounding voxels of the sparse grid*/
			void GetNeighbours(size_t p_fromPoint, std::vector<std::vector<size_t>*> &p_cellValues, std::vector<size_t> &p_neighbours, int p_numberOfNeighbours) {				
				p_neighbours.clear(); //Clear the list
				std::vector<float> distances = std::vector<float>(p_numberOfNeighbours, 100); 				
				int distIdx = 0;
				float crtDist = 0.0f;				
				__m128 a,b;
				__m128 res;
				const int mask = 0xF8;				
				b = _mm_load_ps(m_pointCloud->GetPointAt(p_fromPoint)->position.Element);
				for (std::vector<std::vector<size_t>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {												
						a = _mm_load_ps(m_pointCloud->GetPointAt(*pointIterator)->position.Element);												
						a = _mm_sub_ps(a, b);
						res = _mm_dp_ps(a, a, mask);
						crtDist = res.m128_f32[3];						
						if (crtDist < distances[p_numberOfNeighbours-1]) {
							for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
								if (crtDist <= distances[distIdx]) {								
									distances.insert(distances.begin()+distIdx, crtDist);									
									p_neighbours.insert(p_neighbours.begin()+distIdx, *pointIterator);									
									break;
								}								
							}
						}						
					}
				}				
			}

			/**Calculates the covariance matrix of the first p_numberOfNeighbours points in p_from. Stores it in p_result*/
			void CalcCovMatrix(std::vector<size_t> &p_from, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<size_t>::iterator start = p_from.begin();
				std::vector<size_t>::iterator end;

				if (p_from.size() > p_numberOfNeighbours) end = p_from.begin()+p_numberOfNeighbours;
				else end = p_from.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
								
				for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (m_pointCloud->GetPointAt(*pointIterator))->position.X;
					median.Y += (m_pointCloud->GetPointAt(*pointIterator))->position.Y;
					median.Z += (m_pointCloud->GetPointAt(*pointIterator))->position.Z;
				}
				median /= p_numberOfNeighbours;

				//Now calculate diagonal of cov matrix
				for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X);
					c11 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y);
					c22 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
					c01 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y);
					c02 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
					c12 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

			void CalcCovMatrix(std::vector<Point*> &p_from, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<Point*>::iterator start = p_from.begin();
				std::vector<Point*>::iterator end;

				if (p_from.size() > p_numberOfNeighbours) end = p_from.begin()+p_numberOfNeighbours;
				else end = p_from.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
								
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (*pointIterator)->position.X;
					median.Y += (*pointIterator)->position.Y;
					median.Z += (*pointIterator)->position.Z;
				}
				median /= p_numberOfNeighbours;

				//Now calculate diagonal of cov matrix
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.X - median.X);
					c11 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Y - median.Y);
					c22 += ((*pointIterator)->position.Z - median.Z) * ((*pointIterator)->position.Z - median.Z);
					c01 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Y - median.Y);
					c02 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Z - median.Z);
					c12 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

			void CalcCovMatrix(std::vector<Point*> &p_from0, std::vector<Point*> &p_from1, std::vector<Point*> &p_from2, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<Point*>::iterator pointIterator;
				std::vector<Point*>::iterator start = p_from1.begin();
				std::vector<Point*>::iterator end;

				if (p_from1.size() > p_numberOfNeighbours) end = p_from1.begin()+p_numberOfNeighbours;
				else end = p_from1.end();
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					p_from0.push_back(*pointIterator);
				}

				start = p_from2.begin();
				if (p_from2.size() > p_numberOfNeighbours) end = p_from2.begin()+p_numberOfNeighbours;
				else end = p_from2.end();
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					p_from0.push_back(*pointIterator);
				}
							
				start = p_from0.begin();
				if (p_from0.size() > p_numberOfNeighbours*3) end = p_from0.begin()+(p_numberOfNeighbours*3);
				else end = p_from0.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
				int pointCount = 0;
								
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (*pointIterator)->position.X;
					median.Y += (*pointIterator)->position.Y;
					median.Z += (*pointIterator)->position.Z;
					pointCount++;
				}

				median /= pointCount;

				//Now calculate diagonal of cov matrix
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.X - median.X);
					c11 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Y - median.Y);
					c22 += ((*pointIterator)->position.Z - median.Z) * ((*pointIterator)->position.Z - median.Z);
					c01 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Y - median.Y);
					c02 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Z - median.Z);
					c12 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

		};		

	}
}
									